

#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<unistd.h>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/imgproc/types_c.h>
#include <opencv2/imgcodecs/legacy/constants_c.h>
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<iomanip>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include<thread>
#include <list>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include  "include/stereo_camera_parameter.h"
using namespace cv;
using namespace std;
void LoadImages(const string &strFile, vector<string> &vstrImageFilenames,
                vector<double> &vTimestamps);
                
//depth mm                
void depthToPoints(
	const float * depthparam_param_after_rotate,
	const int max_depth_th,  const uint16_t * pDepthBuff, const int depth_width ,const int depth_height,
	cv::Point3f * pPoints, int & point_num)
{
	int pt_num = 0;
	float fx,fy,cx,cy;
	fx = depthparam_param_after_rotate[0];
	fy = depthparam_param_after_rotate[1];
	cx = depthparam_param_after_rotate[2];
	cy = depthparam_param_after_rotate[3];

	float max_depth_th_m = max_depth_th / 1000.f;
	for(int r_d=0;r_d < depth_height;r_d++){
		for(int c_d=0;c_d < depth_width;c_d++){
			float depth_now = pDepthBuff[r_d*depth_width+c_d]/1000.f;
			if(depth_now > max_depth_th_m || depth_now < 0.2){
				continue;
			}
			
			pPoints[pt_num].x = (c_d - cx)*depth_now/fx;
			pPoints[pt_num].y = (r_d - cy)*depth_now/fy;
			pPoints[pt_num].z = depth_now;
			
			pt_num++;
		}
	}
	point_num = pt_num;
					cout<<" pt_num "<<pt_num<<endl;

}

//rgb_instrinscs_ fx,fy,cx,cy,
//rgb_distdistortion: k0,k1,k2,k3
// rgbPoints: pixel_x, pixel_y, z_in_rgb_coor_m
void pointsToRgb(cv::Mat left_rgb_img, const cv::Point3f * pPoints, const int & point_num,
	const float * rgb_instrinscs_, const float *rgb_distortion,
	const cv::Mat & Tpoint2rgb, cv::Point3f * rgbPoints){
	//trans to rgb
	
	//trans to pixel
	float fx,fy,cx,cy;
	std::vector<cv::Point2f> undistorted;
	fx = rgb_instrinscs_[0]; fy = rgb_instrinscs_[1]; cx = rgb_instrinscs_[2]; cy = rgb_instrinscs_[3];
  //
	for(int iN = 0; iN< point_num; iN++){
		undistorted.push_back(cv::Point2f(pPoints[iN].x / pPoints[iN].z, pPoints[iN].y / pPoints[iN].z));
	}
	
	
	//distort
	float k0,k1,k2,k3;
  k0 = rgb_distortion[0]; k1 = rgb_distortion[1]; k2 = rgb_distortion[2]; k3=rgb_distortion[3];

	std::vector<cv::Point2f> distorted;
	cv::Mat K = (cv::Mat_<double>(3,3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);
  cv::Mat D = (cv::Mat_<double>(1,4) << k0, k1, k2, k3);
  std::cout << K <<std::endl;
  std::cout << D << std::endl;

  K.convertTo(K, CV_64FC1);
  D.convertTo(D, CV_64FC1);

	cv::fisheye::distortPoints(undistorted, distorted, K, D);
	
	{
    //show on origin left rgb
    cv::Mat rgbShow ;
    cv::cvtColor(left_rgb_img, rgbShow, COLOR_GRAY2RGB);

    for(int iN = 0; iN< point_num; iN++)
    {
      float u0 = distorted[iN].x;
      float v0 = distorted[iN].y;

      rgbShow.at<cv::Vec3b>(v0, u0) = cv::Vec3b(255,0,0);
    }
    imshow("origin_leftir_with_point", rgbShow); waitKey(-1);
    imwrite("origin_leftir_with_point.jpg",rgbShow);
	}

  for(int iN = 0; iN< point_num; iN++){
      rgbPoints[iN].x = distorted[iN].x;
      rgbPoints[iN].y = distorted[iN].y;
      rgbPoints[iN].z = pPoints[iN].z;
  }


	cout<<"------ K "<< K <<" D "<<D<<endl;
	
	//try undistort img 
	cv::Mat undistorted_img, newK;
  if(0)
  {
      Mat newimage;
      Mat imageInput = left_rgb_img.clone();

      Mat mapx, mapy,K_new;
      cv::Size new_image_size=left_rgb_img.size();
      initUndistortRectifyMap(K,D,Mat(),K_new,new_image_size,CV_32FC1,mapx,mapy);
      remap(imageInput, newimage, mapx, mapy, CV_INTER_AREA, BORDER_CONSTANT, 255);
      cout<<" newimage "<<newimage.type()<<" "<< newimage.size()<<endl;
      imshow("newimage", newimage); waitKey(-1);
      imwrite("remap.jpg",newimage);
      imwrite("origin.jpg",imageInput);      


      new_image_size=cv::Size(left_rgb_img.cols*2, left_rgb_img.rows*2);
      initUndistortRectifyMap(K,D,Mat(),K_new,new_image_size,CV_32FC1,mapx,mapy);
      remap(imageInput, newimage, mapx, mapy, CV_INTER_AREA, BORDER_CONSTANT, 255);
      cout<<" newimage "<<newimage.type()<<" "<< newimage.size()<<endl;
      imshow("newimage", newimage); waitKey(-1);
      imwrite("remap_2.jpg",newimage);
      imwrite("origin_2.jpg",imageInput);
  }

  {
      Mat imageInput = left_rgb_img.clone();

      cv::Size new_image_size=left_rgb_img.size();
      cv::fisheye::estimateNewCameraMatrixForUndistortRectify(K,D,new_image_size,
                                                              cv::Matx33d::eye(),newK);
      fisheye::undistortImage(left_rgb_img, undistorted_img, K, D, newK);

      cout<<" newimage "<<undistorted_img.type()<<" "<< undistorted_img.size()<<endl;
      imshow("newimage", undistorted_img); waitKey(-1);
      imwrite("undistorted_img.jpg",undistorted_img);
      imwrite("origin.jpg",imageInput);

      cv::Mat rgbShow ;
      cv::cvtColor(undistorted_img, rgbShow, COLOR_GRAY2RGB);

      float fx_new = newK.at<double>(0,0);
      float fy_new = newK.at<double>(1,1);
      float cx_new = newK.at<double>(0,2);
      float cy_new = newK.at<double>(1,2);
      cout<<" newK: "<<newK<<endl;
      cout<<" fx_new "<<fx_new<<" fy_new "<<fy_new<<" cx_new "<<cx_new<<" cy_new "<<cy_new<<endl;
      //blue show depth
      for(int iN = 0; iN< point_num; iN++)
      {
          float x0 = pPoints[iN].x;
          float y0 = pPoints[iN].y;
          float z0 = pPoints[iN].z;

          float u0 = x0 / z0 * fx_new + cx_new;
          float v0 = y0 / z0 * fy_new + cy_new;

          rgbShow.at<cv::Vec3b>(v0, u0) = cv::Vec3b(255,0,0);
      }
          imshow("undistort_leftir_with_point", rgbShow); waitKey(-1);
          imwrite("undistort_leftir_with_point.jpg",rgbShow);


  }

}

//rgb_instrinscs_ fx,fy,cx,cy,
//rgb_distdistortion: k0,k1,k2,k3
// rgbPoints: pixel_x, pixel_y, z_in_rgb_coor_m
void undistortImageNative(cv::Mat left_rgb_img,
    const float * rgb_instrinscs_, const float *rgb_distortion){
    //trans to rgb

    //trans to pixel
    float fx,fy,cx,cy;
    fx = rgb_instrinscs_[0]; fy = rgb_instrinscs_[1]; cx = rgb_instrinscs_[2]; cy = rgb_instrinscs_[3];



    //distort
    float k0,k1,k2,k3;
    k0 = rgb_distortion[0]; k1 = rgb_distortion[1]; k2 = rgb_distortion[2]; k3=rgb_distortion[3];

    std::vector<cv::Point2f> distorted;
    cv::Mat K = (cv::Mat_<double>(3,3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);
    cv::Mat D = (cv::Mat_<double>(1,4) << k0, k1, k2, k3);

    K.convertTo(K, CV_64FC1);
    D.convertTo(D, CV_64FC1);

    //cv::fisheye::distortPoints(undistorted, distorted, K, D);
    cout<<" K "<< K <<" D "<<D<<endl;

    //try undistort img
    cv::Mat undistorted_img, newK;
    fisheye::undistortImage(left_rgb_img, undistorted_img, K, D);
    cout<<" left_rgb_img "<<left_rgb_img.type()<<" "<< left_rgb_img.size()<<endl;

    cout<<" undistorted_img "<<undistorted_img.type()<<endl;
    imshow("undistorted_img", undistorted_img); waitKey(-1);

    {
        Mat newimage;
        Mat imageInput = left_rgb_img.clone();

        Mat mapx, mapy,K_new;
        cv::Size new_image_size=left_rgb_img.size();
        initUndistortRectifyMap(K,D,Mat(),K_new,new_image_size,CV_32FC1,mapx,mapy);
        remap(imageInput, newimage, mapx, mapy, CV_INTER_AREA, BORDER_CONSTANT, 255);
        cout<<" newimage "<<newimage.type()<<" "<< newimage.size()<<endl;
        imshow("newimage", newimage); waitKey(-1);
        imwrite("remap.jpg",newimage);
        imwrite("origin.jpg",imageInput);


        new_image_size=cv::Size(left_rgb_img.cols*2, left_rgb_img.rows*2);
        initUndistortRectifyMap(K,D,Mat(),K_new,new_image_size,CV_32FC1,mapx,mapy);
        remap(imageInput, newimage, mapx, mapy, CV_INTER_AREA, BORDER_CONSTANT, 255);
        cout<<" newimage "<<newimage.type()<<" "<< newimage.size()<<endl;
        imshow("newimage", newimage); waitKey(-1);
        imwrite("remap_2.jpg",newimage);
        imwrite("origin_2.jpg",imageInput);
    }

}



cv::Mat showPointOnRgb(cv::Mat left_img, const int depth_show_th_mm, const cv::Point3f * pPoints, const int & point_num){
	cv::Mat rgbShow ;
	cv::cvtColor(left_img, rgbShow, COLOR_GRAY2RGB);
	

	
	//blue show depth
	float show_depth_th_m = depth_show_th_mm / 1000.f;
	for(int iN = 0; iN< point_num; iN++){
		int ux = pPoints[iN].x;
		int uy = pPoints[iN].y;
		
		float weight = pPoints[iN].z / show_depth_th_m;
		weight = max(0.f, min(1.f, weight));
		
		//rgbShow.at<cv::Vec3b>(uy, ux)(2) = max(255, (int)(weight*255));
				rgbShow.at<cv::Vec3b>(uy, ux) = cv::Vec3b(255,0,0);

		
	}
		imshow("point_left_img", rgbShow); waitKey(10);
        imwrite("point_on_leftir.jpg", rgbShow);


}

string exit_fileName="/tmp/interruput_nv";
int main(int argc, char **argv)
{

  //instrincs_test(); return 1;
  std::string main_path = "/media/ai/0bea6433-71ce-4bf1-a689-3b0348c3c57b/vslam/0423/20250409080003_92965";

  //load yaml
  // string yamlFile = string(main_path)+"/camera_params.yml";
  string yamlFile = main_path +"/camera_params.yml";

  cerr<<" yamlFile "<<yamlFile<<endl;


  //  stereoCameraParameters paras;
  //  loadParameter(yamlFile, paras);
  //  return 0;

  vector<string> vstrImageFilenames;
  vector<double> vTimestamps;//ms

  string strFile = string(main_path)+"/left_ir.txt";
  LoadImages(strFile, vstrImageFilenames, vTimestamps);
        
        
  vector<string> vstrImageFilenames_right;
  vector<double> vTimestamps_right;//ms

  strFile = string(main_path)+"/right_ir.txt";
  LoadImages(strFile, vstrImageFilenames_right, vTimestamps_right);
  
  
  vector<string> vstrDepthFilenames;
  vector<double> vDepthTimestamps;//ms

  string strDepthFile = string(main_path)+"/depth.txt";
  LoadImages(strDepthFile, vstrDepthFilenames, vDepthTimestamps);
  
  cout<<" depth file num "<< vstrDepthFilenames.size()<<" rgb file size "<< vstrImageFilenames.size()<<endl;
        
  int width = 1024;
  int height= 768;
  // float rgb_instrinscs[]={380.977051, 380.876495, 518.386719, 337.40976};
  // float rgb_distortion[]={0.045951847, -0.011027799, 0.007337844, -0.00242689601};

  // float rgb_instrinscs[]={380.514984, 380.354584, 507.075226, 338.767242};
  // float rgb_distortion[]={0.0422173887, -0.000108877211, -0.000979754375, -0.000220517832};
  float rgb_instrinscs[]={380.514984, 380.059082, 507.075226, 338.767242};
  float rgb_distortion[]={0.0422173887, -0.000108877211, -0.000979754375, -0.000220517832};
        
	int yuv_byte_num = width*height*1.5;

  uchar * temp_buff =(uchar*)malloc(yuv_byte_num);		
  int depth_width = 640;
  int depth_height = 480;

	uint16_t  pDepthBuff[depth_width*depth_height];//mm
  // float depthparam_param_after_rotate[] = {161.301956, 161.301956, 334.934448, 201.649963, -54.1767044};
  float depthparam_param_after_rotate[] = {160.431152, 160.431152, 315.454376, 185.347549, -54.9447289};
  int matched_depth_idx = 0;
  double depth_diff_rgb_max_ms = 0;
  for(int ni=0; 0&&ni<vstrImageFilenames_right.size(); ni++)
  {
    string img_name = string(main_path)+"/"+vstrImageFilenames_right[ni];
    //im = cv::imread(img_name,CV_LOAD_IMAGE_UNCHANGED);
    //cout<<" try open file "    <<img_name<<endl;
    FILE *fp = fopen(img_name.c_str(),"rb");
    if(fp==NULL)
    {
        cout<<" can't open file "    <<img_name<<endl;
        continue;
    }
    cv::Mat fisheye_image = imread(img_name,CV_LOAD_IMAGE_UNCHANGED),fisheye_image_gray, fisheye_image2;
    if(fisheye_image.empty())
    {
      size_t read_size = fread(temp_buff, 1, yuv_byte_num, fp);
      fclose(fp);
      if(read_size == yuv_byte_num)
      {
        fisheye_image = cv::Mat(height+height/2, width, CV_8UC1, temp_buff);
        fisheye_image_gray = cv::Mat(height, width, CV_8UC1, temp_buff);
        cv::cvtColor(fisheye_image, fisheye_image2, COLOR_YUV2BGR_NV12);
        
        //imwrite(img_name, fisheye_image);					
      }
      else
      {
        cout<<img_name<<" read_size "<<read_size<<" "<<yuv_byte_num<<endl;
        continue;
      }
    }
			imshow("right_fisheye_image_gray ",fisheye_image_gray);
      waitKey(10);
      imshow("right_fisheye_image_gray2 ",fisheye_image2); 
      waitKey(10);
  }



  for(int ni=0; ni<vstrImageFilenames.size(); ni++)
  {
    string img_name = string(main_path)+"/"+vstrImageFilenames[ni];
    //im = cv::imread(img_name,CV_LOAD_IMAGE_UNCHANGED);
    //cout<<" try open file "    <<img_name<<endl;
    FILE *fp = fopen(img_name.c_str(),"rb");
    if(fp==NULL)
    {
        cout<<" can't open file "    <<img_name<<endl;
        continue;
    }
    cv::Mat fisheye_image = imread(img_name,CV_LOAD_IMAGE_UNCHANGED),fisheye_image_gray;
    if(fisheye_image.empty())
    {
      size_t read_size = fread(temp_buff, 1, yuv_byte_num, fp);
      fclose(fp);
      if(read_size == yuv_byte_num)
      {
        fisheye_image = cv::Mat(height+height/2, width, CV_8UC1, temp_buff);
        fisheye_image_gray = cv::Mat(height, width, CV_8UC1, temp_buff);
        //cv::cvtColor(fisheye_image, fisheye_image2, COLOR_YUV2BGR_NV12);
        
        //imwrite(img_name, fisheye_image);					
      }
      else
      {
        cout<<img_name<<" read_size "<<read_size<<" "<<yuv_byte_num<<endl;
        continue;
      }
        //cout<<img_name<<" read_size "<<read_size<<" "<<yuv_byte_num<<endl;

    }
    imshow("left_fisheye_image_gray ",fisheye_image_gray);
    if(ni == vstrImageFilenames.size()-1)
    {
      waitKey(-1);
    }
    waitKey(10);
		//cout<<img_name<<" "<<(float)((float)ni/(float)vstrImageFilenames.size())<<endl;
    if(access(exit_fileName.c_str(),F_OK)==0)
    {
      cerr<<" exit_fileName "<<exit_fileName<<" checked, finish  "<<endl;
      return 0;
    }
    //undistortImageNative(fisheye_image_gray, rgb_instrinscs, rgb_distortion);
						
		//if get rgb ok, try depth show
		double t_delta = 1000;



    #if 1
    while(matched_depth_idx < vstrDepthFilenames.size())//1ms
    {
      t_delta = fabs(vDepthTimestamps[matched_depth_idx]-vTimestamps[ni]);
      if(t_delta < 30){
        cout<<fixed<<setprecision(5)<<" matched_depth_idx: "<<matched_depth_idx<<" t_delta "<<t_delta<<" vTimestamps[i] "<<vTimestamps[ni]<<" vstrDepthFilenames[matched_depth_idx] "<< vstrDepthFilenames[matched_depth_idx]<<endl;
        break;
      }	
      //cout<<fixed<<setprecision(5)<<" matched_depth_idx: "<<matched_depth_idx<<" t_delta "<<t_delta<<" vTimestamps[i] "<<vTimestamps[ni]<<" vstrDepthFilenames[matched_depth_idx] "<< vstrDepthFilenames[matched_depth_idx]<<endl;

      matched_depth_idx++;
    }

    if(t_delta<30){
      std::cout << "t_delta: " << t_delta << std::endl;
      //show depth
      string img_name_depth = string(main_path)+"/"+vstrDepthFilenames[matched_depth_idx];
      FILE *fp_d = fopen(img_name_depth.c_str(),"rb");
      if(fp_d==NULL)
      {
        cout<<" can't open file "    <<img_name_depth<<endl;
        continue;
      }
      //cout<<" open file "    <<img_name_depth <<" pDepthBuff "<<sizeof(pDepthBuff)<<endl;
      size_t read_size_depth = fread(pDepthBuff, 1, depth_width*depth_height*2, fp_d);									
      cout<<" open file "    <<img_name_depth<<endl;
      //cout<<" open file "    <<img_name_depth<<" read_size_depth "<<read_size_depth <<endl;
      fclose(fp_d);
      if(read_size_depth != depth_width*depth_height*2){
        cout<<" read depth raw file failed read_size_depth "<<read_size_depth<<endl;
        continue;
      }
      cv::Mat depth_image = cv::Mat(depth_height, depth_width, CV_16UC1, pDepthBuff);
      cv::Mat showDepth(depth_height, depth_width, CV_8UC1);
      //max min value
      uint16_t max_d = 0, min_d = 65536;
      for(int r_d=0;r_d < depth_height;r_d++){
        for(int c_d=0;c_d < depth_width;c_d++){
          max_d = max(pDepthBuff[r_d*depth_width+c_d], max_d);
          min_d = min(pDepthBuff[r_d*depth_width+c_d], min_d);
          float depth_now = pDepthBuff[r_d*depth_width+c_d]/1000.f;
          if(depth_now < 2 && depth_now > 0.2){
            ;//cout<<" r_d "<<r_d<<" c_d "<<c_d<<" depth "<< depth_now<<endl;
          }
          int value_gray = depth_now*255/5;//5m->255
          showDepth.data[r_d*depth_width+c_d]=min(255, max(value_gray, 0));
        }
      }
      //cout<<" max_d "<<max_d<<" min_d "<<min_d<<endl;
      depth_diff_rgb_max_ms = max(depth_diff_rgb_max_ms, t_delta);
      imshow("showDepth",showDepth);waitKey(1);
      cv::Point3f points[depth_width*depth_height];
      int points_num =0;
      depthToPoints(depthparam_param_after_rotate, 7000, pDepthBuff, depth_width, depth_height, points, points_num);
      
      cv::Point3f rgbPoints[points_num];
      pointsToRgb(fisheye_image_gray, points, points_num, rgb_instrinscs, rgb_distortion, cv::Mat(), rgbPoints);
      
      cout<< "points_num "<<points_num<<endl;
      /*show on rgb-left*/
      showPointOnRgb(fisheye_image_gray, 7000, rgbPoints, points_num);
    }
    else
      cout<<fixed<<setprecision(5)<<" failed matched_depth_idx: "<<matched_depth_idx<<" t_delta "<<t_delta<<" vTimestamps[i] "<<vTimestamps[ni]<<endl;
  #endif

  }
  waitKey(-1);
  cout<<" depth_diff_rgb_max_ms "<<depth_diff_rgb_max_ms<<endl;
  return 0;
}




void LoadImages(const string &strFile, vector<string> &vstrImageFilenames, vector<double> &vTimestamps)
{
  ifstream f;
  f.open(strFile.c_str());
  const double std_interval_ms = 100;
  double diff_thresh_ms = 10;

  double timestamp_last = -1;
  while(!f.eof())
  {
      string s;
      getline(f,s);
      if(!s.empty())
      {
          stringstream ss;
          ss << s;
          int t1, t2;
          double timestamp;
          string sRGB;
          ss >> timestamp;
          vTimestamps.push_back(timestamp);
          ss >> t1>>t2;
          ss >> sRGB;
          vstrImageFilenames.push_back(sRGB);
          if(timestamp_last>0){
      double diff0 = timestamp - timestamp_last;
      double error0 = fabs(diff0-std_interval_ms);
      if(error0 >=  diff_thresh_ms){
        cout<<fixed<<setprecision(5)<<" rgb diff big timestamp "<<timestamp<<" diff0 "<<diff0<<endl;

      }
      
      
    }
    timestamp_last = timestamp;
          //cout<<" timestamp "<<timestamp<<" "<<t1<<" "<<t2<<" img_name: "<<sRGB<<endl;
      }
  }
}



#if 0
typedef struct {
  unsigned char r;
  unsigned char g;
  unsigned char b;
}MYRGB;
MYRGB * m_colortable=NULL;
static int minDepth = 0;
static int maxDepth = 10;
int nvpfm_getyuvfromindex(int index, unsigned char *py, unsigned char *pu, unsigned char *pv) {
  if (index < 255 && index >= 0) {
    *py = m_colortable[index].r;
    *pu = m_colortable[index].g;
    *pv = m_colortable[index].b;
    return 0;
  } else {
    return -1;
  }
}

void calculatecolortable() {
  if (NULL == m_colortable) {
    m_colortable = (MYRGB *)calloc(1, sizeof(MYRGB) * 65536);
  }
  memset(m_colortable, 0, sizeof(MYRGB) * 65536);

  for (int i = (int)minDepth; i < 65536; i++) { // 0.2m-5m
    if (i > (int)maxDepth) {
      unsigned char y = 0;
      unsigned char u = 0;
      unsigned char v = 0;
      int result = nvpfm_getyuvfromindex(254, &y, &u, &v);
      float fr = y + 1.4075 * (v - 128);

      float fg = y - 0.3455 * (u - 128) - 0.7169 * (v - 128);

      float fb = y + 1.779 * (u - 128);
      /*		static FILE* fp = fopen("table.txt", "wt");
      fprintf(fp,"table:%f,%f,%f\n", fr, fg, fb);
      fflush(fp);
      static FILE* fp = fopen("table.txt", "wt");
      fprintf(fp, ">max,:%f,%f,%f\n", fr, fg, fb);
      fflush(fp);*/
      m_colortable[i].r = (unsigned char)fr;
      m_colortable[i].g = (unsigned char)fg;
      m_colortable[i].b = (unsigned char)fb;
      continue;
    }
    int effectindex = (int)((float)(i - (int)minDepth) * 255.0 / (float)(maxDepth - minDepth));
    //	if (effectindex > 255)effectindex = 255;
    if (effectindex == 255)
      effectindex = 254;

    unsigned char y = 0;
    unsigned char u = 0;
    unsigned char v = 0;
    int result = nvpfm_getyuvfromindex(effectindex, &y, &u, &v);
    // y = 26; u = 170; v = 122;
    if (result >= 0) {
      float fr = y + 1.4075 * (v - 128);

      float fg = y - 0.3455 * (u - 128) - 0.7169 * (v - 128);

      float fb = y + 1.779 * (u - 128);
      /*		static FILE* fp = fopen("table.txt", "wt");
      fprintf(fp, "index:%d,:%f,%f,%f\n", effectindex, fr, fg, fb);
      fflush(fp);
      */
      m_colortable[i].r = (unsigned char)fr;
      m_colortable[i].g = (unsigned char)fg;
      m_colortable[i].b = (unsigned char)fb;
    }
  }
}
void compute_depth2pseudo(uint16_t *depthdata, int width, int height, uint8_t *pseudo, uint16_t max_d, uint16_t min_d, bool rgborbgr) {
  if (max_d == 0 && min_d == 0) {
    if (m_colortable == NULL) {
      calculatecolortable();
    }
  } else {
    if (m_colortable == NULL || (maxDepth != max_d || minDepth != min_d)) {
      maxDepth = max_d;
      minDepth = min_d;
      calculatecolortable();
    }
  }

  for (int row = 0; row < height; row++) {
    for (int col = 0; col < width; col++) {
      int index = row * width + col;
      uint16_t distance = *(depthdata + row * width + col);
      MYRGB *color = (m_colortable + distance);

      pseudo[index * 3] = (rgborbgr ? color->r : color->b);     // info->cr;
      pseudo[index * 3 + 1] = color->g;                         // info->cg;
      pseudo[index * 3 + 2] = (rgborbgr ? color->b : color->r); // info->cb;
    }
  }
}
#endif

